function MoveServo(id, pos, speed)
%{  
    DualDynamixel180Phase will rotate two dynamixels
    in opposite directions.
    
    is is a two element array that contains the
    Dynamixel IDs

    theta is the angle in radians to move from center

    speed is the moving speed 0-1023
    
    DualDynamixel180Phase(id, sind(0), 256)

    Sample SyncWrite command
    FF FF FE 0E 83 1E 04 01 00 02 00 02 02 00 01 00 01 45
%}
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\import');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\bin');
    %Load the dynamixel library
    loadlibrary('dynamixel','dynamixel.h');
    DEFAULT_PORTNUM = SetPort;  % COM3
    DEFAULT_BAUDNUM = 1;  % 1Mbps
    
    response = calllib('dynamixel','dxl_initialize',DEFAULT_PORTNUM ,DEFAULT_BAUDNUM);
    pause on
    

    
    %There should only be 2 Dynamixels total
    numberOfDynamixels = length(id);
    
    if response == 1
        
        
        %Broadcast ID
        calllib('dynamixel','dxl_set_txpacket_id', 254);
        
        %Length is 14
        %That handles position and speed for two dynamixels
        calllib('dynamixel','dxl_set_txpacket_length',14);
        
        %Write instruction
        calllib('dynamixel','dxl_set_txpacket_instruction',131);
        
        %Starting address (goal position)
        calllib('dynamixel','dxl_set_txpacket_parameter',0, 30);
        
        %length of data to write to each dynamixel
        %We're writing position and speed = 4 bytes
        calllib('dynamixel','dxl_set_txpacket_parameter',1, 4);
        
        %Parameters for syncwrite
        % id | position | speed
        %ID 
        calllib('dynamixel','dxl_set_txpacket_parameter',2, id);
        
        lowByte = calllib('dynamixel','dxl_get_lowbyte', pos);
        highByte = calllib('dynamixel','dxl_get_highbyte', pos);
        calllib('dynamixel','dxl_set_txpacket_parameter',3, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',4, highByte);
        
        %Speed = 512
        lowByte = calllib('dynamixel','dxl_get_lowbyte', speed);
        highByte = calllib('dynamixel','dxl_get_highbyte', speed);
        calllib('dynamixel','dxl_set_txpacket_parameter',5, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',6, highByte);
        
        
       
        %transmit
        calllib('dynamixel','dxl_tx_packet');
        
    else
        disp('Failed to open USB2Dynamixel!');
    end
    
    
    calllib('dynamixel','dxl_terminate');
    unloadlibrary('dynamixel');
    
    